function [optima, EvaluationNum, GenerationNum, optima_v2] = emommo_test_countifnotclose(Problem, Algorithm, OptSet)
%INPUT
%	Problem.
%       Dimension: scalar, the dimension of the decision variables
%       LowerBound: 1 -by- d vector, the lower bound of the decision variables
%       UpperBound: 1 -by- d vector, the upper bound of the decision variables
%       ObjFunc: the function handle of the objective funtions,
%           y = ObjFunc(x); x: n -by- d matrix; y: n -by- 1 vector
%   Algorithm.
%       PopulationSize: scalar, the population size
%       F: the mutation factor of the differential evolution
%       CR: crossover index of the differential evolution
%       scale: the scale factor of the modified objective function
%       MaxFEs: the maximal budget
%OUTPUT
%   fval: the optimal value
%   sol: the optimal solution
%   EvaluationNum: the total number of evalutions used
%   
%%
% PROBLEM SETTING
d = Problem.Dimension;
LB = Problem.LowerBound;
UB = Problem.UpperBound;
ObjFunc = Problem.ObjFunc;
% ALGORITHM ASSIGNMENT
PopulationSize = Algorithm.PopulationSize;
CutRatio = Algorithm.CutRatio;
MaxGenerationNum = Algorithm.MaxGenerationNum;
% the optimal set for testing purpose
if ~isempty(OptSet)
    OptSetRemain = OptSet.sol;
end
N = Algorithm.MaxFEs;
%% INITIALIZATION
landscape = zeros(N,d+1);
GenerationNum = 0;  % the current generation number
% population = rand([PopulationSize,d]).*repmat((UB-LB),[PopulationSize,1])+repmat(LB,[PopulationSize,1]);
population = lhsdesign(PopulationSize,d).*repmat((UB-LB),[PopulationSize,1])+repmat(LB,[PopulationSize,1]);
fitness = ObjFunc(population);
EvaluationNum = PopulationSize;  % the budget used
% max_fitness = max(fitness);
% min_fitness = min(fitness);
% remove the found optimum
population = [zeros(PopulationSize,2),fitness,zeros(PopulationSize,1),population];  % [front, crowded rank, fitnesses, d_grid, solution]
landscape(1:EvaluationNum,:) = population(:,[3,5:(4+d)]);
population_merge = zeros(2*PopulationSize, 4+d);
population_merge(1:PopulationSize,:) = population;
%% OPTIMIZATION
while GenerationNum<MaxGenerationNum
    population_merge((PopulationSize+1):end,:) = 0;
    offspring = generate_offspring(population(:,5:end));
    population_merge((PopulationSize+1):end,5:end) = offspring;
    fitness = ObjFunc(offspring);
    population_merge((PopulationSize+1):end,3) = fitness;
    landscape((EvaluationNum+1):(EvaluationNum+PopulationSize),:) = population_merge((PopulationSize+1):end,[3,5:(4+d)]);
    EvaluationNum = EvaluationNum+PopulationSize;
    population_merge(:,4) = modified_obj(population_merge(:,5:end));
    % ranking
    population_merge = non_dominated_sort(population_merge);
    population_merge = crowded_comparison(population_merge);
    [~,rank_id] = sort(population_merge(:,2));
    population_merge = population_merge(rank_id,:);
    population = population_merge(1:PopulationSize,:);
    GenerationNum = GenerationNum+1;
end
landscape = landscape(1:EvaluationNum,:);
%% Peak Detection
% initial cut
landscape = unique(landscape,'rows');
minf = min(landscape(:,1));
maxf = max(landscape(:,1));
landscape(landscape(:,1)>=(minf+CutRatio*(maxf-minf)),:) = [];
N_remain = size(landscape, 1); 
if(N_remain > 5000)
    sel_idx = randperm(N_remain);
    sel_idx = sel_idx(1:min(5000, length(sel_idx)));
    landscape = landscape(sel_idx,:);
    minf = min(landscape(:,1));
end
% peak detection
peaks = [];
while(~isempty(landscape))
    peaks0 = peak_detection(landscape);
    peaks = [peaks, peaks0];
    maxf = max(landscape(:,1));
    landscape(landscape(:,1)>=(minf+0.5*(maxf-minf)),:) = [];
end
peakNum = length(peaks);
centers = zeros(peakNum,d+1);
for k = 1:peakNum
    peak = peaks{k};
    [~, I] = min(peak(:,1));
    centers(k,:) = peak(I,:);
end
% remove redundant solutions
[centers, ia, ~] = unique(centers,'rows');
peaks = peaks(ia);

%% local search
peakNum = size(centers,1); 
fes = floor((N-EvaluationNum)/peakNum);
optima = zeros(peakNum,d+1);
for k = 1:peakNum
    offset =  0.025*(UB - LB);
    lbounds = max([LB;centers(k,2:end)-offset]);
    ubounds = min([UB;centers(k,2:end)+offset]);
    % local optimization using CSO
    seedx = centers(k,2:end);
    [bestx, bestf] = CSO(seedx, ObjFunc, fes, lbounds, ubounds);
    optima(k,:) = [bestf,bestx];
end

optima(optima(:,1)>(OptSet.fval+OptSet.epsilon),:)=[];
optima_v2 = [];
[~,id] = sort(optima(:,1));
optima = optima(id,:);
optima_v2 = optima(1,:);
for k = 2:size(optima,1)
    dis = sqrt(sum((repmat(optima(k,2:end),[size(optima_v2,1),1])-optima_v2(:,2:end)).^2,2));
    min_dis = min(dis);
    if min_dis>OptSet.radius
        optima_v2 = [optima_v2;optima(k,:)];
    end
end
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FUNCTIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function d_grid = modified_obj(x)
        PopulationSize_f = size(x,1);
        min_xi = min(x,[],1);
        max_xi = max(x,[],1);
        min_xi = repmat(min_xi,[PopulationSize_f,1]);
        max_xi = repmat(max_xi,[PopulationSize_f,1]);
        xx = floor((PopulationSize_f/2-1).*(x-min_xi)./(max_xi-min_xi));
        distance = pdist2(xx, xx, 'minkowski', 1);
        distance(1:(PopulationSize_f+1):end) = inf;
        maxmin_dis = max(min(distance));
        distance(1:(PopulationSize_f+1):end) = 0;
        delta = (1-(GenerationNum-1)/MaxGenerationNum)*maxmin_dis;
        niche = (distance<delta);
        d_grid = sum(niche.*distance./delta,2);
        d_grid = d_grid - sum(niche,2);
        d_grid = -d_grid;
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [ offspring ] = generate_offspring(population)
        ProC = 0.9;       %Crossover probability
        ProM = 0.01;     %Mutation probability
        DisC = 50;   	%Crossover index
        DisM = 20;   	%Mutation index
        PopulationSize_f = size(population,1);
        MatingPool = population(randperm(PopulationSize_f),:);
        %SBX
        offspring = zeros(PopulationSize_f,d);
        for i = 1 : 2 : PopulationSize_f
            beta = zeros(1,d);
            mu  = rand(1,d);
            beta(mu<=0.5) = (2*mu(mu<=0.5)).^(1/(DisC+1));
            beta(mu>0.5)  = (2-2*mu(mu>0.5)).^(-1/(DisC+1));
            beta = beta.*(-1).^randi([0,1],1,d);
            beta(rand(1,d)>ProC) = 1;
            offspring(i,:)   = (MatingPool(i,:)+MatingPool(i+1,:))/2+beta.*(MatingPool(i,:)-MatingPool(i+1,:))/2;
            offspring(i+1,:) = (MatingPool(i,:)+MatingPool(i+1,:))/2-beta.*(MatingPool(i,:)-MatingPool(i+1,:))/2;
        end
        %Polynomial Mutation
        MaxValue = repmat(UB,[PopulationSize_f,1]);
        MinValue = repmat(LB,[PopulationSize_f,1]);
        kk = rand(PopulationSize_f,d);
        mu = rand(PopulationSize_f,d);
        Temp = kk<=ProM & mu<0.5;
        offspring(Temp) = offspring(Temp)+(MaxValue(Temp)-MinValue(Temp)).*((2.*mu(Temp)+(1-2.*mu(Temp)).*(1-(offspring(Temp)-MinValue(Temp))./(MaxValue(Temp)-MinValue(Temp))).^(DisM+1)).^(1/(DisM+1))-1);
        Temp = kk<=ProM & mu>=0.5; 
        offspring(Temp) = offspring(Temp)+(MaxValue(Temp)-MinValue(Temp)).*(1-(2.*(1-mu(Temp))+2.*(mu(Temp)-0.5).*(1-(MaxValue(Temp)-offspring(Temp))./(MaxValue(Temp)-MinValue(Temp))).^(DisM+1)).^(1/(DisM+1)));
        offspring(offspring>UB) = MaxValue(offspring>UB);
        offspring(offspring<LB) = MinValue(offspring<LB);
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [population] = non_dominated_sort(population)
    % Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan. A Fast
    %   Elitist Multi-objective Genetic Algorithm: NSGA-II. IEEE Transactions
    %   on Evolutionary Computation, 6(2):182-197, April 2002.
    %population: [non-dominated rank, fitnesses, objective function, solution]
        population(:,1) = 0;
        population_size_f = size(population,1);
        SS = zeros(population_size_f,population_size_f);  % the solutions that be dominated by p
        Front_f = zeros(population_size_f,population_size_f);  % the non-dominated front
        NumF = zeros(population_size_f+1,1);
        dominate_or_not = zeros(population_size_f,population_size_f);
        for p = 1:population_size_f
            temp = (sum(repmat(population(p,3:4),[population_size_f,1])<=population(:,3:4),2)==2)...
                .*(sum(repmat(population(p,3:4),[population_size_f,1])<population(:,3:4),2)>0);
            dominate_or_not(p,:) = temp';
        end
        NumSS = sum(dominate_or_not==1,2); % the number of solutions that be dominated by p
        nn = (sum(dominate_or_not==1,1))'; % the number of solutions that dominate p
        currentFront = find(nn==0);  % the first front
        nn(currentFront) = -1;
        population(currentFront,1) = 1;
        NumF(1) = length(currentFront);
        Front_f(1,1:NumF(1)) = currentFront;
        for p = 1:population_size_f
            SS(p,1:NumSS(p)) = find(dominate_or_not(p,:)==1);
        end
        ii = 1;
        while NumF(ii)>0
            for jj = 1:NumF(ii)
                p = Front_f(ii,jj);
                q = SS(p,1:NumSS(p));
                nn(q) = nn(q)-1;
            end
            currentFront = find(nn==0);  % the first front
            nn(currentFront) = -1;
            population(currentFront,1) = ii+1;
            NumF(ii+1) = length(currentFront);
            Front_f(ii+1,1:NumF(ii+1)) = currentFront;
            ii = ii+1;
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [population] = crowded_comparison(population)
    % Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan. A Fast
    %   Elitist Multi-objective Genetic Algorithm: NSGA-II. IEEE Transactions
    %   on Evolutionary Computation, 6(2):182-197, April 2002.
    %population: [non-dominated rank, crowded rank, fitnesses, solution]
        maxobj = max(population(:,3:4));
        minobj = min(population(:,3:4));
        max_front = max(population(:,1));
        rank_finished = 0;
        for ii = 1:max_front
            solutionId_f = find(population(:,1)==ii);
            fitness_temp = population(solutionId_f,3:4);
            solutionN_f = length(solutionId_f);
            distance = zeros(solutionN_f,1);
            for jj = 1:2
                [~,rank_f] = sort(fitness_temp(:,jj));
                distance(rank_f(1)) = inf;
                distance(rank_f(end)) = inf;
                for kk = 2:(solutionN_f-1)
                    distance(rank_f(kk)) = distance(rank_f(kk))...
                        +(fitness_temp(rank_f(kk+1),jj)-fitness_temp(rank_f(kk-1),jj))/(maxobj(jj)-minobj(jj));
                end
            end
            [~,rank_f] = sort(distance,'descend');
            population(solutionId_f(rank_f),2) = (1:solutionN_f)'+rank_finished;
            rank_finished = rank_finished+solutionN_f;
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [peaks] = peak_detection(landscape)
        landscapeNum = size(landscape,1);
        peaks = cell(0,0);
        II = zeros(landscapeNum,1);
        archive_x = normc(landscape(:,2:end));
        distance = pdist2(archive_x, archive_x, 'minkowski', 1);
        distance(logical(eye(landscapeNum))) = 1e20;
        % locate peaks
        K = 0; B = ones(1,landscapeNum);
        while(sum(B) > 1)
            K = K + 1;
            one_peak = [];
            % threshhold calculation
            MinDat = min(distance, [], 2);
            sigma = max(MinDat(MinDat ~= 1e20));
            % locate the one to start
            starter = find(MinDat == sigma, 1, 'first');
            neighbors = starter;
            %    neighbors = find(B, 1, 'first');
            while(~isempty(neighbors))
                % add the first element to the curren peak
                one_peak = [one_peak; landscape(neighbors(1),:)];
                II(neighbors(1)) = K;
                % discover new neighbors of the first element
                new_neighbors = find(distance(neighbors(1),:) <= max(sigma, 1e-3) & distance(neighbors(1),:) > 0);
                % disable connections of solutions in new neighborhood
                distance(neighbors,new_neighbors) = 1e20;
                distance(new_neighbors, neighbors) = 1e20;
                distance(new_neighbors, new_neighbors) = 1e20;
                % add new neighrbors
                neighbors = [neighbors, new_neighbors];
                % remove the first element from the neighbors and B
                B(neighbors(1)) = 0;
                neighbors(1) = [];
            end
            distance(II == K, :) = 1e20;
            distance(:, II == K) = 1e20;
            peaks(K) = {one_peak};
        end
        % last peak
        if(sum(B) == 1)
            K = K + 1;
            idx = find(B);
            II(idx) = K;
            peaks(K) = {landscape(idx,:)};
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function remove_opt( new_samples )
    % remove_opt: remove the found optimal solution from OptSetRemain
        new_samples(new_samples(:,1)>=(OptSet.fval+OptSet.epsilon),:) = [];
        for ii = 1:size(new_samples,1)
            OptDis = sqrt(sum((repmat(new_samples(ii,2:end),[size(OptSetRemain,1),1]) - OptSetRemain).^2,2));
            [MinDis, OptId] = min(OptDis);
            if MinDis<OptSet.radius
                OptSetRemain(OptId,:) = [];
            end
        end
    end
end

